#include <ctime>
#include "objectList.h"


ObjectList::ObjectList(ros::NodeHandle& nh, std::string publish_topic)
{
    //Min distance between toys.
    min_distance = 0.15;
    //Field of view.
    setFOV(std::make_pair(0.4,0.2), std::make_pair(0.4,-0.2),
           std::make_pair(1.5,-0.6), std::make_pair(1.5,0.6));

    mNode = nh;
    pub_topic = publish_topic;
    list_publisher = mNode.advertise<group6::DetectedObjectArray>(publish_topic, 50);
    objectArray.header.frame_id = "/map";

    //Add forbidden zones (Baskets).
    addForbidden(0.2, -1.3, 0.17);
    addForbidden(3.3, -0.38, 0.17);
}


void ObjectList::addForbidden(double x, double y, double r)
{
    object.x = x;
    object.y = y;
    object.z = r;

    forbidden_zones.objects.push_back(object);
}

void ObjectList::updateObjectList(const group6::DetectedObjectArray::ConstPtr& detectedObjects)
{
    mListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1));
    //Get robot's current pose.
    mListener.lookupTransform("/map","base_link",ros::Time(0), currentTransform);
    a_r = tf::getYaw(currentTransform.getRotation());
    x_r = currentTransform.getOrigin().x();
    y_r = currentTransform.getOrigin().y();

    //Clear new object list.
    newObjectArray.objects.clear();


    //---Way 1: Add all currently seen objects to the list.
    //newObjectArray.objects = detectedObjects.get()->objects;

    //---Way 2: Add detected objects to the new list, if possible.
    for(int i = 0; i < detectedObjects.get()->objects.size(); i++)
    {
        if((distanceCheck(detectedObjects.get()->objects[i])))// && (notForbidden(detectedObjects.get()->objects[i])))
        {
            newObjectArray.objects.push_back(detectedObjects.get()->objects[i]);
        }
    }


    //Add old objects to the new list, if possible.
    for(int i = 0; i < objectArray.objects.size(); i++)
    {
        if(distanceCheck(objectArray.objects[i]))
        {
            if(!( shouldBeSeen(objectArray.objects[i])))
            newObjectArray.objects.push_back(objectArray.objects[i]);
        }
    }

    //Update the object list.
    objectArray.objects = newObjectArray.objects;

    //Publish, dont publish empty list.
    publishObjects(false);

    //Print object list info.
    //printObjectList(detectedObjects.get()->objects.size());
}

void ObjectList::publishObjects(bool publish_empty)
{
    objectArray.size = objectArray.objects.size();
    objectArray.header.stamp = ros::Time::now();

    //Publish if there are objects to publish.
    if( (objectArray.size > 0) || (publish_empty) )
    {
        list_publisher.publish(objectArray);
    }
}

bool ObjectList::shouldBeSeen(group6::DetectedObject newObject)
{
    //Calculate object location in robot coordinates.
    x_or = (newObject.x-x_r) * cos(a_r) + (newObject.y-y_r) * sin(a_r);
    y_or = - (newObject.x-x_r) * sin(a_r) + (newObject.y-y_r) * cos(a_r);

    if((x_or < (x_min.first * y_or + x_min.second)) || (x_or > (x_max.first * y_or + x_max.second)))
        return false;
    if((y_or < (y_min.first * x_or + y_min.second)) || (y_or > (y_max.first * x_or + y_max.second)))
        return false;

    return true;
}


void ObjectList::setFOV(std::pair<double, double> xy1, std::pair<double, double> xy2,
            std::pair<double, double> xy3, std::pair<double, double> xy4)
{
    x_min = solveXline(xy1, xy2);
    x_max = solveXline(xy3, xy4);
    y_min = solveYline(xy2, xy3);
    y_max = solveYline(xy1, xy4);
}


bool ObjectList::distanceCheck(group6::DetectedObject newObject)
{
    //Check the distance to all other objects.
    for(int i = 0; i < newObjectArray.objects.size(); i++)
    {
        if(sqrt( pow(newObject.x-newObjectArray.objects[i].x, 2) +
                 pow(newObject.y-newObjectArray.objects[i].y, 2)) <= min_distance)
            return false;
    }
    return true;
}

bool ObjectList::notForbidden(group6::DetectedObject newObject)
{
    //Check the distance to all forbiddne points.
    for(int i = 0; i < forbidden_zones.objects.size(); i++)
    {
        if(sqrt( pow(newObject.x-forbidden_zones.objects[i].x, 2) +
                 pow(newObject.y-forbidden_zones.objects[i].y, 2)) <= forbidden_zones.objects[i].z)
            return false;
    }
    return true;
}

std::pair<double,double> ObjectList::solveXline(std::pair<double,double> xy1, std::pair<double,double> xy2)
{
    line.first = (xy2.first-xy1.first)/(xy2.second-xy1.second);
    line.second = xy1.first-xy1.second*line.first;
    return line;
}

std::pair<double,double> ObjectList::solveYline(std::pair<double,double> xy1, std::pair<double,double> xy2)
{
    line.first = (xy2.second-xy1.second)/(xy2.first-xy1.first);
    line.second = xy1.second-xy1.first*line.first;
    return line;
}


void ObjectList::printObjectList(int detectedObstaclesAmount)
{
    //Update current time.
    time_now = time(0);
    time_string = ctime(&time_now);
    std::cout << "--- " << time_string;

    //Print general info.
    std::cout << "Detected/Deleted: " << detectedObstaclesAmount << ", ";
    std::cout << "total object amount: " << objectArray.objects.size() << ", ";

    //Print each toy info.
    for(int i = 0; i < objectArray.objects.size(); i++)
    {
        std::cout << "-Object " << i+1 << ": ID = " << objectArray.objects[i].object_id <<
                     ", X = " << objectArray.objects[i].x << ", Y = " << objectArray.objects[i].y << ", ";
    }
    std::cout << "\n\n";
}


void ObjectList::deleteObjects(const geometry_msgs::Vector3::ConstPtr& deletePoint)
{
    newObjectArray.objects.clear();
    del_distance = deletePoint.get()->z;

    //Deleted objects counter.
    deleted_objects = 0;

    for(int i = 0; i < objectArray.size; i++)
    {
        distance = sqrt( pow(objectArray.objects[i].x - deletePoint.get()->x, 2) +
                 pow(objectArray.objects[i].y - deletePoint.get()->y, 2));

        if(distance > del_distance)
        {
            newObjectArray.objects.push_back(objectArray.objects[i]);
        }
        else
        {
            deleted_objects++;
        }
    }

    objectArray.objects = newObjectArray.objects;

    if(deleted_objects > 0)
    {
        //Publish new list, publish also empty.
        publishObjects(true);
    }

    //Print object list info.
    //printObjectList(deleted_objects);
}

int main(int argc, char** argv){
//Basic params.
ros::init(argc, argv, "objectList");
ros::NodeHandle n;

//Create object list instance.
ObjectList oblist(n, "/object_list");

//Update list always when objects detected.
ros::Subscriber mDetectedObjectsSubscriber = n.subscribe("detected_object_array", 10,
                                                         &ObjectList::updateObjectList, &oblist);

//Delete objects according to the delete point.
ros::Subscriber mDeleteObjectsSubscriber = n.subscribe("/delete_objects", 10,
                                                         &ObjectList::deleteObjects, &oblist);

ros::spin();
}
